Finding Lane Lines (Advanced)¶
Compute the camera calibration matrix and distortion coefficients given a set of chessboard images¶
I start by preparing "object points", which will be the (x, y, z) coordinates of the chessboard corners in the world. Here I am assuming the chessboard is fixed on the (x, y) plane at z=0, such that the object points are the same for each calibration image. Thus, objp is just a replicated array of coordinates, and objpoints will be appended with a copy of it every time I successfully detect all chessboard corners in a test image. imgpoints will be appended with the (x, y) pixel position of each of the corners in the image plane with each successful chessboard detection. The chessboard images along with the found corners are output by the code snippet below.
In [1]:
import numpy as np
import cv2
import glob
import matplotlib.pyplot as plt
%matplotlib inline
#shape of the matrix including all cross-over points
crossover_matrix_shape=(9,6)
# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.zeros((crossover_matrix_shape[1]*crossover_matrix_shape[0],3), np.float32)
objp[:,:2] = np.mgrid[0:9, 0:6].T.reshape(-1,2)
# Arrays to store object points and image points from all the images.
objpoints = [] # 3d points in real world space
imgpoints = [] # 2d points in image plane.
# Make a list of calibration images
cal_images = glob.glob('camera_cal/calibration*.jpg')
# Step through the list and search for chessboard corners
for idx, fname in enumerate(cal_images):
img = cv2.imread(fname)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
f, (ax1, ax2) = plt.subplots(1, 2, figsize=(20,10))
ax1.imshow(img)
ax1.set_title('Original chessboard', fontsize=30)
# Find the chessboard corners
ret, corners = cv2.findChessboardCorners(gray,
crossover_matrix_shape,
flags = cv2.CALIB_CB_ADAPTIVE_THRESH)
# If found, add object points, image points
if ret == True:
objpoints.append(objp)
imgpoints.append(corners)
# Draw and display the corners
corners = cv2.drawChessboardCorners(img, (9,6), corners, ret)
ax2.imshow(corners)
ax2.set_title('Corners', fontsize=30)
cv2.waitKey(500)
plt.show()
cv2.destroyAllWindows()
Apply a distortion correction to raw images¶
Next, the objpoints and imgpoints are used to compute the camera calibration and distortion coefficients using the cv2.calibrateCamera() function. The distortion correction is then applied to the test image using the cv2.undistort() function, and the undistorted test images are output by the codesnippet below.
In [2]:
import pickle
%matplotlib inline
undistorted = []
#images = glob.glob('camera_cal/calibration*.jpg')
images = glob.glob('test_images/test*.jpg')
# Test undistortion on an image
for idx ,image_path in enumerate(images):
img = cv2.imread(image_path)
img_size = (img.shape[1], img.shape[0])
# Do camera calibration given object points and image points
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, img_size,None,None)
dst = cv2.undistort(img, mtx, dist, None, mtx)
undistorted.append(dst)
# Visualize undistortion
f, (ax1, ax2) = plt.subplots(1, 2, figsize=(20,10))
ax1.imshow(img)
ax1.set_title('Original Image', fontsize=30)
ax2.imshow(dst)
ax2.set_title('Undistorted Image', fontsize=30)
plt.show()
Use color transforms, gradients, etc., to create a thresholded binary image.¶
In the following pipeine, the undistorted test images are first transformed to HLS space, of which the s_channel is used to create a binary threshold mask. This mask is then combined with a scobel-filter mask along the x and y axis, to detect the edges in the images. The selected filters and the threshold values were found empirically and proved to generate good results.
In [3]:
import numpy as np
import cv2
import matplotlib.pyplot as plt
import matplotlib.image as mpimg
# Edit this function to create your own pipeline.
def transform(img, s_thresh=(170, 255), sx_thresh=(20, 100)):
img = np.copy(img)
# Convert to HSV color space and separate the V channel
hls = cv2.cvtColor(img, cv2.COLOR_RGB2HLS).astype(np.float)
l_channel = hls[:,:,1]
s_channel = hls[:,:,2]
# Sobel x
sobelx = cv2.Sobel(l_channel, cv2.CV_64F, 1, 0) # Take the derivative in x
abs_sobelx = np.absolute(sobelx) # Absolute x derivative to accentuate lines away from horizontal
scaled_sobel = np.uint8(255*abs_sobelx/np.max(abs_sobelx))
# Threshold x gradient
sxbinary = np.zeros_like(scaled_sobel)
sxbinary[(scaled_sobel >= sx_thresh[0]) & (scaled_sobel <= sx_thresh[1])] = 1
# Threshold color channel
s_binary = np.zeros_like(s_channel)
s_binary[(s_channel >= s_thresh[0]) & (s_channel <= s_thresh[1])] = 1
# Stack each channel
# Note color_binary[:, :, 0] is all 0s, effectively an all black image. It might
# be beneficial to replace this channel with something else.
color_binary = np.dstack(( np.zeros_like(sxbinary), sxbinary, s_binary))
combined_binary = np.zeros_like(sxbinary)
combined_binary[(s_binary == 1) | (sxbinary == 1)] = 1
return combined_binary
transformed = []
for image in undistorted:
result = transform(image)
transformed.append(result)
# Plot the result
f, (ax1, ax2) = plt.subplots(1, 2, figsize=(24, 9))
f.tight_layout()
ax1.imshow(image)
ax1.set_title('Original Image', fontsize=40)
ax2.imshow(result,cmap='gray')
ax2.set_title('Pipeline Result', fontsize=40)
plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.)
Apply a perspective transform to rectify binary image ("birds-eye view")¶
The following warps the test images to a bird-eye view using the cv2.getPerspectiveTransform and the cv2.warpPerspective functions. Based on a source and a destination trapezoid, cv2.getPerspectiveTransform generates a transformation matrix that is used by cv2.warpPerspective to transform the perspective of the initial image according to the source/destination mapping. The source and destination trapezoids were found empirically, by using a test image with straight lines and defining the source and destination points such that the lines in the transformed image are parallel.
In [4]:
top_right_src = [740, 460]
top_left_src = [570, 460]
bottom_right_src = [1200, 705]
bottom_left_src = [155, 705]
top_right_dest = [870, 100]
top_left_dest = [155, 100]
bottom_right_dest = [870, 705]
bottom_left_dest = [155, 705]
# For source points I'm grabbing the outer four detected corners
src = np.float32([top_right_src,
bottom_right_src,
bottom_left_src,
top_left_src])
# For destination points, I'm arbitrarily choosing some points to be
# a nice fit for displaying our warped result
# again, not exact, but close enough for our purposes
dst = np.float32([top_right_dest,
bottom_right_dest,
bottom_left_dest,
top_left_dest])
# Define a function that takes an image, number of x and y points,
# camera matrix and distortion coefficients
def corners_unwarp(img, src, dst):
# Grab the image shape
img_size = (img.shape[1], img.shape[0])
# Given src and dst points, calculate the perspective transform matrix
M = cv2.getPerspectiveTransform(src, dst)
# Warp the image using OpenCV warpPerspective()
warped = cv2.warpPerspective(img, M, img_size)
return warped, M
warped = []
for image in transformed:
img_size = (image.shape[1], image.shape[0])
# Do camera calibration given object points and image points
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints,
imgpoints,
img_size,
None,
None)
warped_ret = corners_unwarp(image, src = src, dst = dst)
if warped_ret is not None:
warped_image = warped_ret[0]
warped.append(warped_image)
write_name = 'warped/warped'+str(idx)+'.jpg'
cv2.imwrite(write_name, dst)
#Visualize transformation
f, (ax1, ax2) = plt.subplots(1, 2, figsize=(20,10))
ax1.imshow(image, cmap='gray')
ax1.set_title('Original Image', fontsize=30)
ax2.imshow(warped_image, cmap='gray')
ax2.set_title('Warped Image', fontsize=30)
plt.show()
Detect lane pixels and fit to find the lane boundary¶
The find_lane_fits function below takes the warped and transformed images, and calculates windowed histograms to detect the pixels forming a lane. It then calculates a 2nd order polinomial to fit the pixels formin the left and right lanes. The lane fits are then passed to the draw_lanes function, which generates an images where the lanes are marked.
In [5]:
import numpy as np
import cv2
import matplotlib.pyplot as plt
margin = 100
def find_lane_fits(binary_warped, margin = 100, minpix = 50 ):
# Assuming you have created a warped binary image called "binary_warped"
# Take a histogram of the bottom half of the image
histogram = np.sum(binary_warped[binary_warped.shape[0]/2:,:], axis=0)
# Create an output image to draw on and visualize the result
out_img = np.dstack((binary_warped, binary_warped, binary_warped))*255
# Find the peak of the left and right halves of the histogram
# These will be the starting point for the left and right lines
midpoint = np.int(histogram.shape[0]/2)
leftx_base = np.argmax(histogram[:midpoint])
rightx_base = np.argmax(histogram[midpoint:]) + midpoint
# Choose the number of sliding windows
nwindows = 9
# Set height of windows
window_height = np.int(binary_warped.shape[0]/nwindows)
# Identify the x and y positions of all nonzero pixels in the image
nonzero = binary_warped.nonzero()
nonzeroy = np.array(nonzero[0])
nonzerox = np.array(nonzero[1])
# Current positions to be updated for each window
leftx_current = leftx_base
rightx_current = rightx_base
# Create empty lists to receive left and right lane pixel indices
left_lane_inds = []
right_lane_inds = []
# Step through the windows one by one
for window in range(nwindows):
# Identify window boundaries in x and y (and right and left)
win_y_low = binary_warped.shape[0] - (window+1)*window_height
win_y_high = binary_warped.shape[0] - window*window_height
win_xleft_low = leftx_current - margin
win_xleft_high = leftx_current + margin
win_xright_low = rightx_current - margin
win_xright_high = rightx_current + margin
# Draw the windows on the visualization image
cv2.rectangle(out_img,(win_xleft_low,win_y_low),(win_xleft_high,win_y_high),(0,255,0), 2)
cv2.rectangle(out_img,(win_xright_low,win_y_low),(win_xright_high,win_y_high),(0,255,0), 2)
# Identify the nonzero pixels in x and y within the window
good_left_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) & (nonzerox >= win_xleft_low) & (nonzerox < win_xleft_high)).nonzero()[0]
good_right_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) & (nonzerox >= win_xright_low) & (nonzerox < win_xright_high)).nonzero()[0]
# Append these indices to the lists
left_lane_inds.append(good_left_inds)
right_lane_inds.append(good_right_inds)
# If you found > minpix pixels, recenter next window on their mean position
if len(good_left_inds) > minpix:
leftx_current = np.int(np.mean(nonzerox[good_left_inds]))
if len(good_right_inds) > minpix:
rightx_current = np.int(np.mean(nonzerox[good_right_inds]))
# Concatenate the arrays of indices
left_lane_inds = np.concatenate(left_lane_inds)
right_lane_inds = np.concatenate(right_lane_inds)
# Extract left and right line pixel positions
leftx = nonzerox[left_lane_inds]
lefty = nonzeroy[left_lane_inds]
rightx = nonzerox[right_lane_inds]
righty = nonzeroy[right_lane_inds]
# Fit a second order polynomial to each
left_fit = np.polyfit(lefty, leftx, 2)
right_fit = np.polyfit(righty, rightx, 2)
return leftx,lefty, left_fit, rightx, righty, right_fit
def draw_lanes(image, left_fit, right_fit):
ploty = np.linspace(0, image.shape[0]-1, image.shape[0] )
left_fitx = left_fit[0]*ploty**2 + left_fit[1]*ploty + left_fit[2]
right_fitx = right_fit[0]*ploty**2 + right_fit[1]*ploty + right_fit[2]
out_img = np.dstack((image, image, image))*255
window_img = np.zeros_like(out_img)
# Generate a polygon to illustrate the search window area
# And recast the x and y points into usable format for cv2.fillPoly()
left_line_window1 = np.array([np.transpose(np.vstack([left_fitx-margin, ploty]))])
left_line_window2 = np.array([np.flipud(np.transpose(np.vstack([left_fitx+margin, ploty])))])
left_line_pts = np.hstack((left_line_window1, left_line_window2))
right_line_window1 = np.array([np.transpose(np.vstack([right_fitx-margin, ploty]))])
right_line_window2 = np.array([np.flipud(np.transpose(np.vstack([right_fitx+margin, ploty])))])
right_line_pts = np.hstack((right_line_window1, right_line_window2))
# Draw the lane onto the warped blank image
cv2.fillPoly(window_img, np.int_([left_line_pts]), (0,255, 0))
cv2.fillPoly(window_img, np.int_([right_line_pts]), (0,255, 0))
return window_img
lanes = []
for image in warped:
f, (ax1, ax2) = plt.subplots(1, 2, figsize=(20,10))
ax1.imshow(image, cmap='gray')
ax1.set_title('Original Image', fontsize=30)
_,_,left_fit,_,_,right_fit = find_lane_fits(image)
result = draw_lanes(image, left_fit, right_fit)
lanes.append(result)
plt.imshow(result)
Determine the curvature of the lane and vehicle position with respect to center¶
The curve radius is calculated based on a formula introduced at https://classroom.udacity.com/nanodegrees/nd013/parts/fbf77062-5703-404e-b60c-95b78b2f3f9e/modules/2b62a1c3-e151-4a0e-b6b6-e424fa46ceab/lessons/40ec78ee-fb7c-4b53-94a8-028c5c60b858/concepts/2f928913-21f6-4611-9055-01744acc344f. Since the original lane fits were calculated in pixel space and the actual curve radius has to be calculated for real world space, the values are transformed based on an estimated meter / pixel ratio.
The position of the car relative to the image center is calculated by first finding the center between the lowest y-positions on the lane and then subtracting this value from the image center. Again, all values are transformed to real world space.
In [6]:
import numpy as np
import matplotlib.pyplot as plt
def get_curve_radius(y_eval, leftx, lefty, rightx, righty):
# Define conversions in x and y from pixels space to meters
ym_per_pix = 30/720 # meters per pixel in y dimension
xm_per_pix = 3.7/700 # meters per pixel in x dimension
# Fit new polynomials to x,y in world space
left_fit_cr = np.polyfit(lefty*ym_per_pix, leftx*xm_per_pix, 2)
right_fit_cr = np.polyfit(righty*ym_per_pix, rightx*xm_per_pix, 2)
# Calculate the new radii of curvature
left_curverad = ((1 + (2*left_fit_cr[0]*y_eval*ym_per_pix
+ left_fit_cr[1])**2)**1.5) / np.absolute(2*left_fit_cr[0])
right_curverad = ((1 + (2*right_fit_cr[0]*y_eval*ym_per_pix
+ right_fit_cr[1])**2)**1.5) / np.absolute(2*right_fit_cr[0])
# Now our radius of curvature is in meters
return left_curverad, right_curverad
def get_car_position_relative_to_center(image, left_fit, right_fit):
xm_per_pix = 3.7/700 # meteres per pixel in x dimension
#Get the x position of the lowest point for each lane
left_pt = left_fit[0]*image.shape[0]**2 + left_fit[1]*image.shape[0] + left_fit[2]
right_pt = right_fit[0]*image.shape[0]**2 + right_fit[1]*image.shape[0] + right_fit[2]
# meters from center
screen_middel_pixel = image.shape[1]/2
car_middle_pixel = int((right_pt + left_pt)/2)
screen_off_center = screen_middel_pixel-car_middle_pixel
meters_off_center = xm_per_pix * screen_off_center
return meters_off_center
for image in warped:
plt.imshow(image, cmap='gray')
plt.show()
leftx, lefty, left_fit, rightx, righty, right_fit = find_lane_fits(image)
left_curverad, right_curverad = get_curve_radius(image.shape[0],
leftx, lefty,
rightx, righty)
print('Radius of left lane: '+str(left_curverad)+' m ;',
'Radius of right lane: '+str(right_curverad)+' m')
position = get_car_position_relative_to_center(image, left_fit, right_fit)
print('Car position relative to center: '+str(position))
Warp the detected lane boundaries back onto the original image¶
The lane-fits are rewarped into the original image perspective by using the corners_unwarp function defined above again and just reversing the source and destination points.
In [7]:
for image in lanes:
img_size = (image.shape[1], image.shape[0])
# Do camera calibration given object points and image points
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints,
imgpoints,
img_size,
None,
None)
warped_ret = corners_unwarp(image, src = dst, dst = src)
if warped_ret is not None:
warped_image = warped_ret[0]
#Visualize transformation
f, (ax1, ax2) = plt.subplots(1, 2, figsize=(20,10))
ax1.imshow(image, cmap='gray')
ax1.set_title('Warped perspective', fontsize=30)
ax2.imshow(warped_image, cmap='gray')
ax2.set_title('Original perspective', fontsize=30)
plt.show()
Output visual display of the lane boundaries and numerical estimation of lane curvature and vehicle position¶
To demonstrate the correctness of the calculations above, the lane-fits are drawn on the original undistorted test images below.
In [8]:
for images in zip(undistorted , lanes):
img_size = (image.shape[1], image.shape[0])
# Do camera calibration given object points and image points
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints,
imgpoints,
img_size,
None,
None)
warped_ret = corners_unwarp(images[1], src = dst, dst = src)
if warped_ret is not None:
warped_image = warped_ret[0]
#Visualize transformation
f, (ax1, ax2) = plt.subplots(1, 2, figsize=(20,10))
ax1.imshow(images[0], cmap='gray')
ax1.set_title('Original image', fontsize=30)
result = cv2.addWeighted(images[0], 1, warped_image, 0.3, 0)
ax2.imshow(result, cmap='gray')
ax2.set_title('Lane boundaries', fontsize=30)
plt.show()
Putting it all together¶
Finally, all calculation steps are combined to a pipeline that can be conveniently used to get the lane boundaries in one go.
In [9]:
def process(image):
img_size = (image.shape[1], image.shape[0])
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, img_size,None,None)
undistorted_image = cv2.undistort(image, mtx, dist, None, mtx)
transformed_image = transform(undistorted_image)
M = cv2.getPerspectiveTransform(src, dst)
warped_image = cv2.warpPerspective(transformed_image, M, img_size)
_,_,left_fit,_,_,right_fit = find_lane_fits(warped_image)
lanes = draw_lanes(warped_image, left_fit, right_fit)
M = cv2.getPerspectiveTransform(dst, src)
unwarped_lanes = cv2.warpPerspective(lanes, M, img_size)
result = cv2.addWeighted( undistorted_image, 1, unwarped_lanes, 0.3, 0)
return result
image_paths = glob.glob('test_images/test*.jpg')
for image_path in image_paths:
image = cv2.imread(image_path)
result = process(image)
plt.imshow(result)
plt.show()
